#ifndef OPTIMIZER_H
#define OPTIMIZER_H

#include "imu_factor.h"
#include "pose_observe.h"
#include "state.h"

#include "ceres_solver_optimize.h"

using namespace std;

class Optimizer
{
public:
	Optimizer();
	void OptimizeGN(State& state, const Eigen::Vector2d &velocity, const Eigen::Vector3d &pose);
	void OptimizeGN(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose);
	void OptimizeLM(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose);
	void OptimizeLMCh6(State& last_state, State& state, const Eigen::Vector3d &last_velocity, const Eigen::Vector3d &velocity, const Eigen::Vector3d &delta_odometry, const Eigen::Vector3d &last_pose, const Eigen::Vector3d &curr_pose, const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt);

private:
	double Chi2(const Eigen::Matrix<double, 5, 1>& residual, const Eigen::Matrix<double, 5, 5>& information_matrix);
	double Chi2(const Eigen::Matrix<double, 2, 1>& residual, const Eigen::Matrix<double, 2, 2>& information_matrix);
	double Chi2(const Eigen::Matrix<double, 3, 1>& residual, const Eigen::Matrix<double, 3, 3>& information_matrix);

	void update_state(State& state, const Eigen::Matrix<double, 8, 1>& dx);
	void update_state(State& last_state, State& state, const Eigen::Matrix<double, 13, 1>& dx);

	void update_current_obs_state(State& imu_state, State& last_imu_state, const State& last_state, const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt);
};

#endif
